// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2010-2011 Hauke Heibel <heibel@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"

#include <unsupported/Eigen/Splines>

namespace Eigen {

// lets do some explicit instantiations and thus
// force the compilation of all spline functions...
template class Spline<double, 2, Dynamic>;
template class Spline<double, 3, Dynamic>;

template class Spline<double, 2, 2>;
template class Spline<double, 2, 3>;
template class Spline<double, 2, 4>;
template class Spline<double, 2, 5>;

template class Spline<float, 2, Dynamic>;
template class Spline<float, 3, Dynamic>;

template class Spline<float, 3, 2>;
template class Spline<float, 3, 3>;
template class Spline<float, 3, 4>;
template class Spline<float, 3, 5>;

}

Spline<double, 2, Dynamic>
closed_spline2d()
{
	RowVectorXd knots(12);
	knots << 0, 0, 0, 0, 0.867193179093898, 1.660330955342408, 2.605084834823134, 3.484154586374428, 4.252699478956276,
		4.252699478956276, 4.252699478956276, 4.252699478956276;

	MatrixXd ctrls(8, 2);
	ctrls << -0.370967741935484, 0.236842105263158, -0.231401860693277, 0.442245185027632, 0.344361228532831,
		0.773369994120753, 0.828990216203802, 0.106550882647595, 0.407270163678382, -1.043452922172848,
		-0.488467813584053, -0.390098582530090, -0.494657189446427, 0.054804824897884, -0.370967741935484,
		0.236842105263158;
	ctrls.transposeInPlace();

	return Spline<double, 2, Dynamic>(knots, ctrls);
}

/* create a reference spline */
Spline<double, 3, Dynamic>
spline3d()
{
	RowVectorXd knots(11);
	knots << 0, 0, 0, 0.118997681558377, 0.162611735194631, 0.498364051982143, 0.655098003973841, 0.679702676853675,
		1.000000000000000, 1.000000000000000, 1.000000000000000;

	MatrixXd ctrls(8, 3);
	ctrls << 0.959743958516081, 0.340385726666133, 0.585267750979777, 0.223811939491137, 0.751267059305653,
		0.255095115459269, 0.505957051665142, 0.699076722656686, 0.890903252535799, 0.959291425205444,
		0.547215529963803, 0.138624442828679, 0.149294005559057, 0.257508254123736, 0.840717255983663,
		0.254282178971531, 0.814284826068816, 0.243524968724989, 0.929263623187228, 0.349983765984809,
		0.196595250431208, 0.251083857976031, 0.616044676146639, 0.473288848902729;
	ctrls.transposeInPlace();

	return Spline<double, 3, Dynamic>(knots, ctrls);
}

/* compares evaluations against known results */
void
eval_spline3d()
{
	Spline3d spline = spline3d();

	RowVectorXd u(10);
	u << 0.351659507062997, 0.830828627896291, 0.585264091152724, 0.549723608291140, 0.917193663829810,
		0.285839018820374, 0.757200229110721, 0.753729094278495, 0.380445846975357, 0.567821640725221;

	MatrixXd pts(10, 3);
	pts << 0.707620811535916, 0.510258911240815, 0.417485437023409, 0.603422256426978, 0.529498282727551,
		0.270351549348981, 0.228364197569334, 0.423745615677815, 0.637687289287490, 0.275556796335168,
		0.350856706427970, 0.684295784598905, 0.514519311047655, 0.525077224890754, 0.351628308305896,
		0.724152914315666, 0.574461155457304, 0.469860285484058, 0.529365063753288, 0.613328702656816,
		0.237837040141739, 0.522469395136878, 0.619099658652895, 0.237139665242069, 0.677357023849552,
		0.480655768435853, 0.422227610314397, 0.247046593173758, 0.380604672404750, 0.670065791405019;
	pts.transposeInPlace();

	for (int i = 0; i < u.size(); ++i) {
		Vector3d pt = spline(u(i));
		VERIFY((pt - pts.col(i)).norm() < 1e-14);
	}
}

/* compares evaluations on corner cases */
void
eval_spline3d_onbrks()
{
	Spline3d spline = spline3d();

	RowVectorXd u = spline.knots();

	MatrixXd pts(11, 3);
	pts << 0.959743958516081, 0.340385726666133, 0.585267750979777, 0.959743958516081, 0.340385726666133,
		0.585267750979777, 0.959743958516081, 0.340385726666133, 0.585267750979777, 0.430282980289940,
		0.713074680056118, 0.720373307943349, 0.558074875553060, 0.681617921034459, 0.804417124839942,
		0.407076008291750, 0.349707710518163, 0.617275937419545, 0.240037008286602, 0.738739390398014,
		0.324554153129411, 0.302434111480572, 0.781162443963899, 0.240177089094644, 0.251083857976031,
		0.616044676146639, 0.473288848902729, 0.251083857976031, 0.616044676146639, 0.473288848902729,
		0.251083857976031, 0.616044676146639, 0.473288848902729;
	pts.transposeInPlace();

	for (int i = 0; i < u.size(); ++i) {
		Vector3d pt = spline(u(i));
		VERIFY((pt - pts.col(i)).norm() < 1e-14);
	}
}

void
eval_closed_spline2d()
{
	Spline2d spline = closed_spline2d();

	RowVectorXd u(12);
	u << 0, 0.332457030395796, 0.356467130532952, 0.453562180176215, 0.648017921874804, 0.973770235555003,
		1.882577647219307, 2.289408593930498, 3.511951429883045, 3.884149321369450, 4.236261590369414,
		4.252699478956276;

	MatrixXd pts(12, 2);
	pts << -0.370967741935484, 0.236842105263158, -0.152576775123250, 0.448975001279334, -0.133417538277668,
		0.461615613865667, -0.053199060826740, 0.507630360006299, 0.114249591147281, 0.570414135097409,
		0.377810316891987, 0.560497102875315, 0.665052120135908, -0.157557441109611, 0.516006487053228,
		-0.559763292174825, -0.379486035348887, -0.331959640488223, -0.462034726249078, -0.039105670080824,
		-0.378730600917982, 0.225127015099919, -0.370967741935484, 0.236842105263158;
	pts.transposeInPlace();

	for (int i = 0; i < u.size(); ++i) {
		Vector2d pt = spline(u(i));
		VERIFY((pt - pts.col(i)).norm() < 1e-14);
	}
}

void
check_global_interpolation2d()
{
	typedef Spline2d::PointType PointType;
	typedef Spline2d::KnotVectorType KnotVectorType;
	typedef Spline2d::ControlPointVectorType ControlPointVectorType;

	ControlPointVectorType points = ControlPointVectorType::Random(2, 100);

	KnotVectorType chord_lengths; // knot parameters
	Eigen::ChordLengths(points, chord_lengths);

	// interpolation without knot parameters
	{
		const Spline2d spline = SplineFitting<Spline2d>::Interpolate(points, 3);

		for (Eigen::DenseIndex i = 0; i < points.cols(); ++i) {
			PointType pt = spline(chord_lengths(i));
			PointType ref = points.col(i);
			VERIFY((pt - ref).matrix().norm() < 1e-14);
		}
	}

	// interpolation with given knot parameters
	{
		const Spline2d spline = SplineFitting<Spline2d>::Interpolate(points, 3, chord_lengths);

		for (Eigen::DenseIndex i = 0; i < points.cols(); ++i) {
			PointType pt = spline(chord_lengths(i));
			PointType ref = points.col(i);
			VERIFY((pt - ref).matrix().norm() < 1e-14);
		}
	}
}

void
check_global_interpolation_with_derivatives2d()
{
	typedef Spline2d::PointType PointType;
	typedef Spline2d::KnotVectorType KnotVectorType;

	const Eigen::DenseIndex numPoints = 100;
	const unsigned int dimension = 2;
	const unsigned int degree = 3;

	ArrayXXd points = ArrayXXd::Random(dimension, numPoints);

	KnotVectorType knots;
	Eigen::ChordLengths(points, knots);

	ArrayXXd derivatives = ArrayXXd::Random(dimension, numPoints);
	VectorXd derivativeIndices(numPoints);

	for (Eigen::DenseIndex i = 0; i < numPoints; ++i)
		derivativeIndices(i) = static_cast<double>(i);

	const Spline2d spline =
		SplineFitting<Spline2d>::InterpolateWithDerivatives(points, derivatives, derivativeIndices, degree);

	for (Eigen::DenseIndex i = 0; i < points.cols(); ++i) {
		PointType point = spline(knots(i));
		PointType referencePoint = points.col(i);
		VERIFY_IS_APPROX(point, referencePoint);
		PointType derivative = spline.derivatives(knots(i), 1).col(1);
		PointType referenceDerivative = derivatives.col(i);
		VERIFY_IS_APPROX(derivative, referenceDerivative);
	}
}

EIGEN_DECLARE_TEST(splines)
{
	for (int i = 0; i < g_repeat; ++i) {
		CALL_SUBTEST(eval_spline3d());
		CALL_SUBTEST(eval_spline3d_onbrks());
		CALL_SUBTEST(eval_closed_spline2d());
		CALL_SUBTEST(check_global_interpolation2d());
		CALL_SUBTEST(check_global_interpolation_with_derivatives2d());
	}
}
